/*
 * Copyright 2025 The Authors
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *      http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *
 *@file    : sliding_window_optimizer.cc
 *@brief   : sliding window optimizer
 *@authors : zhanglei
 *@version : v1.0
 *@data    : 2025/9/13
 *
 */

#include "optimizer/sliding_window_optimizer.h"

namespace optimizer {

SlidingWindowOptimizer::SlidingWindowOptimizer(std::shared_ptr<Frame>& frame_ptr, const Eigen::Matrix3d& intrinsic_matrix, const Eigen::Matrix4d& extrinsic_matrix, const uint8_t& camera_id) : frame_ptr_(frame_ptr), intrinsic_matrix_(intrinsic_matrix), extrinsic_matrix_(extrinsic_matrix), camera_id_(camera_id) {
    sliding_window_size_ = frame_ptr->GetSlidingWindowSize();
}
// add state : R, vx, vy, px, py, bg, ba
void SlidingWindowOptimizer::Solve() {
    if(frame_ptr_->state_queue_.size() <= sliding_window_size_) {
        SolveSlidingWindowNotFul();
    } else {
        SolveSlidingWindowFul();
        // LOG(FATAL) << " sliding windows is full " << std::endl;
    }
}

void SlidingWindowOptimizer::SolveSlidingWindowNotFul() {

    // LOG(INFO) << " before optimize : " << std::endl;
    // for (uint32_t i = 0; i < frame_ptr_->state_queue_.size(); ++i) {
    //     LOG(INFO) << " current idx : " << i << std::endl;
    //     LOG(INFO) << "frame_ptr_->state_queue_[i].R : " << frame_ptr_->state_queue_[i].R.log() << std::endl;
    //     LOG(INFO) << "frame_ptr_->state_queue_[i].v : " << frame_ptr_->state_queue_[i].v.transpose() << std::endl;
    //     LOG(INFO) << "frame_ptr_->state_queue_[i].p : " << frame_ptr_->state_queue_[i].p.transpose() << std::endl;
    //     LOG(INFO) << "frame_ptr_->state_queue_[i].bg : " << frame_ptr_->state_queue_[i].bg << std::endl;
    //     LOG(INFO) << "frame_ptr_->state_queue_[i].ba : " << frame_ptr_->state_queue_[i].ba[0] << ",  " << frame_ptr_->state_queue_[i].ba[1] << std::endl;
    // }

    ceres::Problem problem;
    ceres::LossFunction *loss_function = new ceres::CauchyLoss(1.0);
    uint32_t state_size = frame_ptr_->state_queue_.size();
    double R[state_size][1];
    double speed[state_size][2];
    double pose[state_size][2];
    double bias[state_size][3];
    
    for (uint32_t i = 0; i < state_size; ++i) {
        auto *rot_local_parameterization = new RotationLocalParameterization();

        R[i][0] = frame_ptr_->state_queue_[i].R.log();
        if(is_using_imu_factor_) {
            speed[i][0] = frame_ptr_->state_queue_[i].v[0]; 
            speed[i][1] = frame_ptr_->state_queue_[i].v[1]; 
        }

        pose[i][0] = frame_ptr_->state_queue_[i].p[0];
        pose[i][1] = frame_ptr_->state_queue_[i].p[1];

        if(is_using_imu_factor_) {
            bias[i][0] = frame_ptr_->state_queue_[i].bg; 
            bias[i][1] = frame_ptr_->state_queue_[i].ba[0]; 
            bias[i][2] = frame_ptr_->state_queue_[i].ba[1]; 
        }

        problem.AddParameterBlock(R[i], 1, rot_local_parameterization);
        if(is_using_imu_factor_) {
            problem.AddParameterBlock(speed[i], 2);
        }
        problem.AddParameterBlock(pose[i], 2);
        if(is_using_imu_factor_) {
            problem.AddParameterBlock(bias[i], 3);
        }

        if(i == 0) {
            problem.SetParameterBlockConstant(R[i]);
            if(is_using_imu_factor_) {
                problem.SetParameterBlockConstant(speed[i]);
            }
            problem.SetParameterBlockConstant(pose[i]);
        }
    }

    std::vector<Vector3d> world_points;
    std::map<uint64_t, std::vector<std::pair<uint32_t, Vector2d>>> undistorted_point;
    // std::map<uint64_t, std::vector<std::pair<uint32_t, Vector3d>>> normalized_point;
    std::vector<uint64_t> feature_ids;
    std::vector<std::shared_ptr<Feature>> features_ptr;
    
    if(is_using_visual_factor_) {
        GetVisualFactorData(world_points, undistorted_point, feature_ids, features_ptr);
        // LOG(INFO) << " world_points ===== : " << world_points.size() << std::endl;
    }

    for(uint32_t id = 0; id < world_points.size(); ++id) {
        Vector3d& world_pt = world_points[id]; 
        problem.AddParameterBlock(world_pt.data(), 3);
        
        // problem.SetParameterBlockConstant(world_pt.data());
    }
    
    if(is_using_imu_factor_) {
        for (int i = 0; i < state_size-1; i++) {
            int j = i + 1;
  
            ImuFactor* imu_factor = new ImuFactor(frame_ptr_->imu_preintegration_ptr_queue_[j]);
            problem.AddResidualBlock(imu_factor, nullptr, R[i], speed[i], pose[i], bias[i], R[j], speed[j], pose[j]);
        }
        for (int i = 0; i < state_size-1; i++) {
            int j = i + 1;
  
            ImuBiasFactor* imu_bias_factor = new ImuBiasFactor();
            problem.AddResidualBlock(imu_bias_factor, nullptr, bias[i], bias[j]);
        }
    }


    if(is_using_wheel_factor_) {
        for (int i = 0; i < state_size-1; i++) {
            int j = i + 1;
            WheelFactor* wheel_factor = new WheelFactor(frame_ptr_->wheel_preintegration_ptr_queue_[j]);
            problem.AddResidualBlock(wheel_factor, nullptr, R[i], pose[i], R[j], pose[j]);
        }
    }

    if(is_using_imu_factor_) {
        if(is_using_velocity_factor_) {
            for (int i = 0; i < state_size; i++) {
                VelocityFactor* velocity_factor = new VelocityFactor(frame_ptr_->velocity_queue_[i]);
                problem.AddResidualBlock(velocity_factor, nullptr, speed[i]);
            }
        }
    }

    // LOG(INFO) << " intrinsic_matrix_ :\n " << intrinsic_matrix_ << std::endl;

    for(uint32_t id = 0; id < world_points.size(); ++id) {
        Vector3d& world_pt = world_points[id]; 
        if(undistorted_point.find(id) == undistorted_point.end()) {
            continue;
        }
        auto points = undistorted_point.at(id);
        uint32_t point_idx = 0;
        for(const auto& pt : points) {
            ReprojectionFactor *reprojection_factor = new ReprojectionFactor(pt.second, intrinsic_matrix_, extrinsic_matrix_);
            
            Eigen::Vector2d residual = reprojection_factor->EvaluateResidual(world_pt, R[pt.first][0], Eigen::Vector2d(pose[pt.first][0], pose[pt.first][1]));
            if(residual.norm() > 1.0) {
                ++point_idx;
                continue;
            }
            // reprojection_factor->EvaluateNormalizedResidual(normalized_point.at(id)[point_idx].second.head<2>(), world_pt, R[pt.first][0], Eigen::Vector2d(pose[pt.first][0], pose[pt.first][1]));
            
            LOG(INFO) << " residual ===== : " << residual.transpose() << std::endl;

            problem.AddResidualBlock(reprojection_factor, loss_function, R[pt.first], pose[pt.first], world_pt.data()); 
            
            ++point_idx;
        }
    }

    ceres::Solver::Options options;
    // 禁用内部多线程的关键配置：
    options.num_threads = 1;                  // 总线程数设为1（默认可能为CPU核心数）

    // 可选：禁用某些默认启用的并行优化策略
    options.use_nonmonotonic_steps = false;    // 禁用非单调步长（减少并行需求）

    options.linear_solver_type = ceres::DENSE_SCHUR;
    options.trust_region_strategy_type = ceres::DOGLEG;
    options.max_num_iterations = 1000;

    options.minimizer_progress_to_stdout = false; // 关键：打开控制台输出
    // options.logging_type = ceres::PER_MINIMIZER_ITERATION; // 每次迭代都输出日志

    // LOG(INFO) << " start optimize : " << std::endl;

    ceres::Solver::Summary summary;
    ceres::Solve(options, &problem, &summary);

    LOG(INFO) << summary.BriefReport() << std::endl;

    if(!summary.IsSolutionUsable()) {
        return;
    }
    if (summary.termination_type != ceres::CONVERGENCE) {
        return;
    }
    // LOG(INFO) << " after optimize : " << std::endl;
    for (uint32_t i = 0; i < state_size; ++i) {
    
        frame_ptr_->state_queue_[i].R = SO2(R[i][0]);

        if(is_using_imu_factor_) {
            frame_ptr_->state_queue_[i].v[0] = speed[i][0]; 
            frame_ptr_->state_queue_[i].v[1] = speed[i][1]; 
        }
        frame_ptr_->state_queue_[i].p[0] = pose[i][0];
        frame_ptr_->state_queue_[i].p[1] = pose[i][1];

        if(is_using_imu_factor_) {
            frame_ptr_->state_queue_[i].bg = bias[i][0]; 
            frame_ptr_->state_queue_[i].ba[0] = bias[i][1]; 
            frame_ptr_->state_queue_[i].ba[1] = bias[i][2];    
        }
        // LOG(INFO) << " current idx : " << i << std::endl;
        // LOG(INFO) << "frame_ptr_->state_queue_[i].R : " << frame_ptr_->state_queue_[i].R.log() << std::endl;
        // LOG(INFO) << "frame_ptr_->state_queue_[i].v : " << frame_ptr_->state_queue_[i].v.transpose() << std::endl;
        // LOG(INFO) << "frame_ptr_->state_queue_[i].p : " << frame_ptr_->state_queue_[i].p.transpose() << std::endl;
        // LOG(INFO) << "frame_ptr_->state_queue_[i].bg : " << frame_ptr_->state_queue_[i].bg << std::endl;
        // LOG(INFO) << "frame_ptr_->state_queue_[i].ba : " << frame_ptr_->state_queue_[i].ba[0] << ",  " << frame_ptr_->state_queue_[i].ba[1] << std::endl;

    }
    // updata point 3d
    for (uint32_t i = 0; i < world_points.size(); ++i) {
        frame_ptr_->feature_database_ptr_->UpdateLandmark(feature_ids[i], features_ptr[i], world_points[i]);
    }
    // LOG(INFO) << "frame_ptr_->state_queue_.back() " << frame_ptr_->state_queue_.back().p.transpose() << ", " << frame_ptr_->state_queue_.back().R.log() << std::endl;
}

void SlidingWindowOptimizer::SolveSlidingWindowFul() {
    ceres::Problem problem;
    ceres::LossFunction *loss_function = new ceres::CauchyLoss(1.0);
    uint32_t state_size = frame_ptr_->state_queue_.size();

    // LOG(INFO) << "state_size ========= : " << state_size << std::endl;

    double R[state_size][1];
    double speed[state_size][2];
    double pose[state_size][2];
    double bias[state_size][3];
    for (int i = 0; i < state_size; ++i) {
        auto *rot_local_parameterization = new RotationLocalParameterization();

        R[i][0] = frame_ptr_->state_queue_[i].R.log();

        if(is_using_imu_factor_) {
            speed[i][0] = frame_ptr_->state_queue_[i].v[0]; 
            speed[i][1] = frame_ptr_->state_queue_[i].v[1]; 
        }
        pose[i][0] = frame_ptr_->state_queue_[i].p[0];
        pose[i][1] = frame_ptr_->state_queue_[i].p[1];

        if(is_using_imu_factor_) {
            bias[i][0] = frame_ptr_->state_queue_[i].bg; 
            bias[i][1] = frame_ptr_->state_queue_[i].ba[0]; 
            bias[i][2] = frame_ptr_->state_queue_[i].ba[1]; 
        }
        problem.AddParameterBlock(R[i], 1, rot_local_parameterization);
        if(is_using_imu_factor_) {
            problem.AddParameterBlock(speed[i], 2);
        }
        problem.AddParameterBlock(pose[i], 2);
        if(is_using_imu_factor_) {
            problem.AddParameterBlock(bias[i], 3);
        }
    }

    std::vector<Vector3d> world_points;
    std::map<uint64_t, std::vector<std::pair<uint32_t, Vector2d>>> undistorted_point;
    // std::map<uint64_t, std::vector<std::pair<uint32_t, Vector3d>>> normalized_point;
    std::vector<uint64_t> feature_ids;
    std::vector<std::shared_ptr<Feature>> features_ptr;
    
    if(is_using_visual_factor_) {
        GetVisualFactorData(world_points, undistorted_point, feature_ids, features_ptr);
    }

    // LOG(INFO) << "world_points 0========= : " << world_points.size() << std::endl;

    for(uint32_t id = 0; id < world_points.size(); ++id) {
        Vector3d& world_pt = world_points[id]; 
        problem.AddParameterBlock(world_pt.data(), 3);
    }
    // LOG(INFO) << "world_points 1========= : " << world_points.size() << std::endl;

    if (last_marginalization_info_) {
        // construct new marginlization_factor
        MarginalizationFactor *marginalization_factor = new MarginalizationFactor(last_marginalization_info_);
        problem.AddResidualBlock(marginalization_factor, nullptr,
                                 last_marginalization_parameter_blocks_);
    }

    // LOG(INFO) << "world_points 2========= : " << world_points.size() << std::endl;
    
    if(is_using_imu_factor_) {
        for (int i = 0; i < sliding_window_size_; i++) {
            int j = i + 1;
  
            ImuFactor* imu_factor = new ImuFactor(frame_ptr_->imu_preintegration_ptr_queue_[j]);
            problem.AddResidualBlock(imu_factor, nullptr, R[i], speed[i], pose[i], bias[i], R[j], speed[j], pose[j]);
        }

        // LOG(INFO) << "world_points 3========= : " << world_points.size() << std::endl;

        for (int i = 0; i < sliding_window_size_; i++) {
            int j = i + 1;
  
            ImuBiasFactor* imu_bias_factor = new ImuBiasFactor();
            problem.AddResidualBlock(imu_bias_factor, nullptr, bias[i], bias[j]);
        }
    }
    // LOG(INFO) << "world_points 4========= : " << world_points.size() << std::endl;

    if(is_using_wheel_factor_) {
        for (int i = 0; i < sliding_window_size_; i++) {
            int j = i + 1;
            WheelFactor* wheel_factor = new WheelFactor(frame_ptr_->wheel_preintegration_ptr_queue_[j]);
            problem.AddResidualBlock(wheel_factor, nullptr, R[i], pose[i], R[j], pose[j]);
        }
    }
    // LOG(INFO) << "world_points 5========= : " << world_points.size() << std::endl;
    if(is_using_imu_factor_) {
        if(is_using_velocity_factor_) {
            for (int i = 0; i < sliding_window_size_ + 1; i++) {
                VelocityFactor* velocity_factor = new VelocityFactor(frame_ptr_->velocity_queue_[i]);
                problem.AddResidualBlock(velocity_factor, nullptr, speed[i]);
            }
        }
    }

    // LOG(INFO) << "world_points 6========= : " << world_points.size() << std::endl;

    // LOG(INFO) << " intrinsic_matrix_ :\n " << intrinsic_matrix_ << std::endl;

    for(uint32_t id = 0; id < world_points.size(); ++id) {

        // LOG(INFO) << " id ===== : " << id << std::endl;

        Vector3d& world_pt = world_points[id]; 
        // LOG(INFO) << " world_pt ===== : " << world_pt.transpose() << std::endl;
        // LOG(INFO) << " id ===== : " << id << std::endl;
        if(undistorted_point.find(id) == undistorted_point.end()) {
            continue;
        }
        auto points = undistorted_point.at(id);

        // LOG(INFO) << " points ===== : " << points.size() << std::endl;

        uint32_t point_idx = 0;
        for(const auto& pt : points) {
            ReprojectionFactor *reprojection_factor = new ReprojectionFactor(pt.second, intrinsic_matrix_, extrinsic_matrix_);
            
            // LOG(INFO) << " pt.first ===== : " << pt.first << std::endl;

            Eigen::Vector2d residual = reprojection_factor->EvaluateResidual(world_pt, R[pt.first][0], Eigen::Vector2d(pose[pt.first][0], pose[pt.first][1]));
            
            // LOG(INFO) << " residual ===== : " << residual.transpose() << std::endl;

            if(residual.norm() > 1.0) {
                ++point_idx;
                continue;
            }
            // reprojection_factor->EvaluateNormalizedResidual(normalized_point.at(id)[point_idx].second.head<2>(), world_pt, R[pt.first][0], Eigen::Vector2d(pose[pt.first][0], pose[pt.first][1]));
            
            // LOG(INFO) << " residual ===== : " << id << " , " << pt.first << " , " << residual.transpose() << std::endl;

            problem.AddResidualBlock(reprojection_factor, loss_function, R[pt.first], pose[pt.first], world_pt.data()); 
            
            // LOG(INFO) << " point_idx ===== : " << point_idx << std::endl;

            ++point_idx;
        }
    }
    
    // LOG(INFO) << "world_points ========= : " << world_points.size() << std::endl;

    ceres::Solver::Options options;
    // 禁用内部多线程的关键配置：
    options.num_threads = 1;                  // 总线程数设为1（默认可能为CPU核心数）

    // 可选：禁用某些默认启用的并行优化策略
    options.use_nonmonotonic_steps = false;    // 禁用非单调步长（减少并行需求）

    options.linear_solver_type = ceres::DENSE_SCHUR;
    options.trust_region_strategy_type = ceres::DOGLEG;
    options.max_num_iterations = 1000;

    options.minimizer_progress_to_stdout = false; // 关键：打开控制台输出
    // options.logging_type = ceres::PER_MINIMIZER_ITERATION; // 每次迭代都输出日志

    ceres::Solver::Summary summary;
    ceres::Solve(options, &problem, &summary);

    LOG(INFO) << summary.BriefReport() << std::endl;

    // marginalize 
    MarginalizationInfo *marginalization_info = new MarginalizationInfo();
    if (last_marginalization_info_) {
        std::vector<int> drop_set;

        // LOG(INFO) << " last_marginalization_parameter_blocks_.size() ===== : " << last_marginalization_parameter_blocks_.size() << std::endl;
        
        for (int i = 0; i < static_cast<int>(last_marginalization_parameter_blocks_.size()); i++) {
            if (last_marginalization_parameter_blocks_[i] == R[0] || 
                last_marginalization_parameter_blocks_[i] == speed[0] ||
                last_marginalization_parameter_blocks_[i] == pose[0] ||
                last_marginalization_parameter_blocks_[i] == bias[0]) {
                drop_set.push_back(i);
            }
        }
        // construct new marginlization_factor
        MarginalizationFactor *marginalization_factor = new MarginalizationFactor(last_marginalization_info_);
        ResidualBlockInfo *residual_block_info = new ResidualBlockInfo(marginalization_factor, nullptr,
                                                                       last_marginalization_parameter_blocks_,
                                                                       drop_set);

        marginalization_info->addResidualBlockInfo(residual_block_info);
    }

    
    if(is_using_imu_factor_) {

        if (frame_ptr_->imu_preintegration_ptr_queue_[1]->dt_ < 10.0) {
            ImuFactor* imu_factor = new ImuFactor(frame_ptr_->imu_preintegration_ptr_queue_[1]);
            ResidualBlockInfo *residual_block_info = new ResidualBlockInfo(imu_factor, nullptr,
                                                                       vector<double *>{R[0], speed[0], pose[0], bias[0], R[1], speed[1], pose[1]},
                                                                       vector<int>{0, 2});
            marginalization_info->addResidualBlockInfo(residual_block_info);
        }
    
        ImuBiasFactor* marg_imu_bias_factor = new ImuBiasFactor();
        ResidualBlockInfo *bias_residual_block_info = new ResidualBlockInfo(marg_imu_bias_factor, nullptr,
                                                                    vector<double *>{bias[0], bias[1]},
                                                                    vector<int>{0});

        marginalization_info->addResidualBlockInfo(bias_residual_block_info);
    }

    if(is_using_wheel_factor_) {
        WheelFactor* marg_wheel_factor = new WheelFactor(frame_ptr_->wheel_preintegration_ptr_queue_[1]);
        ResidualBlockInfo *wheel_residual_block_info = new ResidualBlockInfo(marg_wheel_factor, nullptr,
                                                                    vector<double *>{R[0], pose[0], R[1], pose[1]},
                                                                    vector<int>{0,1});

        marginalization_info->addResidualBlockInfo(wheel_residual_block_info);
    }

    if(is_using_imu_factor_) {
        if(is_using_velocity_factor_) {
            VelocityFactor* marg_velocity_factor = new VelocityFactor(frame_ptr_->velocity_queue_[0]);
            ResidualBlockInfo *velocity_residual_block_info = new ResidualBlockInfo(marg_velocity_factor, nullptr,
                                                                    vector<double *>{speed[0]},
                                                                    vector<int>{0});

            marginalization_info->addResidualBlockInfo(velocity_residual_block_info);
        }
    }
    
    // todo visual
    for(uint32_t id = 0; id < world_points.size(); ++id) {
        Vector3d& world_pt = world_points[id]; 
        if(undistorted_point.find(id) == undistorted_point.end()) {
            continue;
        }
        auto points = undistorted_point.at(id);
        // uint32_t point_idx = 0;
        uint32_t init_frame_id = 0;

        for(const auto& pt : points) {
            if(pt.first != init_frame_id) {
                continue;
            }
            ReprojectionFactor *reprojection_factor = new ReprojectionFactor(pt.second, intrinsic_matrix_, extrinsic_matrix_);
            Eigen::Vector2d residual = reprojection_factor->EvaluateResidual(world_pt, R[pt.first][0], Eigen::Vector2d(pose[pt.first][0], pose[pt.first][1]));
            if(residual.norm() > 1.0) {
                continue;
            }
        
            ResidualBlockInfo *residual_block_info = new ResidualBlockInfo(reprojection_factor, loss_function,
                                                                           vector<double *>{R[pt.first], pose[pt.first], world_pt.data()},
                                                                           vector<int>{0,1,2});
            marginalization_info->addResidualBlockInfo(residual_block_info);
        }
    }

    marginalization_info->preMarginalize();

    marginalization_info->marginalize();

    std::unordered_map<long, double *> addr_shift;
    for (int i = 1; i <= sliding_window_size_; i++) {
        addr_shift[reinterpret_cast<long>(R[i])] = R[i - 1];

        if(is_using_imu_factor_) {
            addr_shift[reinterpret_cast<long>(speed[i])] = speed[i - 1];
        }
        addr_shift[reinterpret_cast<long>(pose[i])] = pose[i - 1];
        
        if(is_using_imu_factor_) {
            addr_shift[reinterpret_cast<long>(bias[i])] = bias[i - 1];
        }
    }

    std::vector<double *> parameter_blocks = marginalization_info->getParameterBlocks(addr_shift);
    // LOG(INFO) << "parameter_blocks========= : " << parameter_blocks.size() << std::endl;

    if (last_marginalization_info_) {
        delete last_marginalization_info_;
    }

    last_marginalization_info_ = marginalization_info;
    last_marginalization_parameter_blocks_ = parameter_blocks;

    if(!summary.IsSolutionUsable()) {
        return;
    }
    if (summary.termination_type != ceres::CONVERGENCE) {
        return;
    }

    for (uint32_t i = 0; i < state_size; ++i) {
    
        frame_ptr_->state_queue_[i].R = SO2(R[i][0]);

        if(is_using_imu_factor_) {
            frame_ptr_->state_queue_[i].v[0] = speed[i][0]; 
            frame_ptr_->state_queue_[i].v[1] = speed[i][1]; 
        }

        frame_ptr_->state_queue_[i].p[0] = pose[i][0];
        frame_ptr_->state_queue_[i].p[1] = pose[i][1];

        if(is_using_imu_factor_) {
            frame_ptr_->state_queue_[i].bg = bias[i][0]; 
            frame_ptr_->state_queue_[i].ba[0] = bias[i][1]; 
            frame_ptr_->state_queue_[i].ba[1] = bias[i][2];    
        }
        // LOG(INFO) << " current idx : " << i << std::endl;
        // LOG(INFO) << "frame_ptr_->state_queue_[i].R : " << frame_ptr_->state_queue_[i].R.log() << std::endl;
        // LOG(INFO) << "frame_ptr_->state_queue_[i].v : " << frame_ptr_->state_queue_[i].v.transpose() << std::endl;
        // LOG(INFO) << "frame_ptr_->state_queue_[i].p : " << frame_ptr_->state_queue_[i].p.transpose() << std::endl;
        // LOG(INFO) << "frame_ptr_->state_queue_[i].bg : " << frame_ptr_->state_queue_[i].bg << std::endl;
        // LOG(INFO) << "frame_ptr_->state_queue_[i].ba : " << frame_ptr_->state_queue_[i].ba[0] << ",  " << frame_ptr_->state_queue_[i].ba[1] << std::endl;

    }

    for (uint32_t i = 0; i < world_points.size(); ++i) {
        frame_ptr_->feature_database_ptr_->UpdateLandmark(feature_ids[i], features_ptr[i], world_points[i]);
    }
}

void SlidingWindowOptimizer::GetVisualFactorData(std::vector<Vector3d>& world_points, std::map<uint64_t, std::vector<std::pair<uint32_t, Vector2d>>>& undistorted_point, std::vector<uint64_t>& feature_ids, std::vector<std::shared_ptr<Feature>>& features_ptr) {
    for(const std::pair<uint64_t, std::shared_ptr<Feature>>& feature_database : frame_ptr_->feature_database_ptr_->GetFeatureDatabase()) {
        std::shared_ptr<Feature> feature_ptr = feature_database.second;
        if(feature_ptr == nullptr) {
            continue;
        }        
        Vector3d world_point = feature_ptr->world_point_;

        if((world_point.norm() < 0.3) /*|| (world_point.norm() > 100.0)*/) {
            continue;
        }
        world_points.emplace_back(world_point);
        feature_ids.emplace_back(feature_ptr->feature_id_);
        features_ptr.emplace_back(feature_ptr);
        // LOG(INFO) << "world_point : " << world_point.transpose() << std::endl;

        for(const std::pair<uint8_t, std::vector<FeaturePoint>>& id_feature_ptr : feature_ptr->map_camera_id_and_feature_) {
            if(id_feature_ptr.second.size() < 2) {
                continue;
            }
            uint32_t idx = 0;
            std::vector<uint64_t> frame_ids = feature_ptr->map_camera_and_frame_id_.at(id_feature_ptr.first);
            for(const FeaturePoint& point : id_feature_ptr.second) {
                uint64_t frame_id = frame_ids[idx];
                auto it = std::find(frame_ptr_->frame_ids_queue_.begin(), frame_ptr_->frame_ids_queue_.end(), frame_id);
                // 2. 检查迭代器是否指向 deque 末尾（即未找到）
                if (it == frame_ptr_->frame_ids_queue_.end()) {
                    continue;
                }

                uint32_t state_idx = static_cast<uint32_t>(std::distance(frame_ptr_->frame_ids_queue_.begin(), it));
                
                undistorted_point[world_points.size()-1].push_back(std::make_pair(state_idx, point.undistorted_point));
                // normalized_point[world_points.size()-1].push_back(std::make_pair(state_idx, point.normalized_point));
                ++idx;
            }
        }
    }
}

}
